#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>

static cv::Mat _calc_hist(const cv::Mat& src, int* channels, int dims, int* histSize, const float** ranges) {
    CV_Assert(src.type() == CV_8UC1 || src.type() == CV_8UC3);
    CV_Assert(dims == 1 || dims == 2);
    
    cv::Mat hist;
    if (dims == 1) {
        int hs = histSize[0];
        const float* ranges1 = ranges[0];
        float start = ranges1[0], end = ranges1[1];
        float step = (end - start) / (float)hs;
        hist = cv::Mat::zeros(hs, 1, CV_32FC1);
        cv::Mat mat = src;
        if (mat.channels() > 1) {
            std::vector<cv::Mat> mats;
            cv::split(src, mats);
            mat = mats[channels[0]];
        }
        for (int i = 0; i < mat.rows; i++) {
            for (int j = 0; j < mat.cols; j++) {
                float value = (float)mat.ptr<uchar>(i)[j];
                int idx = 0;
                for (float f = start; f < end; f += step) {
                    if (value >= f && value < f+step) {
                        hist.ptr<float>(idx)[0]++;
                        break;
                    }
                    idx++;
                }
            }
        }
    } else if (dims == 2) {
        int s1 = histSize[0], s2 = histSize[1];
        const float* range1 = ranges[0];
        const float* range2 = ranges[1];
        float start1 = range1[0], end1 = range1[1];
        float start2 = range2[0], end2 = range2[1];
        float step1 = (end1 - start1) / (float)s1;
        float step2 = (end2 - start2) / (float)s2;
        hist = cv::Mat::zeros(s1, s2, CV_32FC1);
        cv::Mat mat1, mat2;
        std::vector<cv::Mat> mats;
        cv::split(src, mats);
        mat1 = mats[channels[0]];
        mat2 = mats[channels[1]];
        for (int i = 0; i < mat1.rows; i++) {
            for (int j = 0; j < mat1.cols; j++) {
                float value1 = (float)mat1.ptr<uchar>(i)[j];
                int idx1 = 0;
                for (float f1 = start1; f1 < end1; f1 += step1) {
                    if (value1 >= f1 && value1 < f1 + step1) {
                        float value2 = (float)mat2.ptr<uchar>(i)[j];
                        int idx2 = 0;
                        for (float f2 = start2; f2 < end2; f2 += step2) {
                            if (value2 >= f2 && value2 < f2 + step2) {
                                hist.ptr<float>(idx1)[idx2] += 1;
                                break;
                            }
                            idx2++;
                        }
                        break;
                    }
                    idx1++;
                }
            }
        }
    }
    
    return hist;
}

int main(int argc, char **argv) {
    cv::Mat src = cv::imread("/home/xlll/Downloads/opencv/samples/data/lena.jpg", cv::IMREAD_UNCHANGED);
    if (src.empty()) {
        std::cout << "failed to read image" << std::endl;
        return EXIT_FAILURE;
    }
    
    int nimages = 1;
    int channels[] = {0};
    cv::Mat mask = cv::Mat();
    int dims = 1;
    int histSize[] = {256};
    float histRange[] = {0, 256};
    const float* ranges[] = {histRange};
    bool uniform = true;
    bool accumulate = false;
    
    cv::Mat hist_b, hist_g, hist_r;
    cv::calcHist(&src, nimages, channels, mask, hist_b, dims, histSize, ranges, uniform, accumulate);
    channels[0] = 1;
    cv::calcHist(&src, nimages, channels, mask, hist_g, dims, histSize, ranges, uniform, accumulate);
    channels[0] = 2;
    cv::calcHist(&src, nimages, channels, mask, hist_r, dims, histSize, ranges, uniform, accumulate);
    
    cv::Mat my_hist = _calc_hist(src, channels, dims, histSize, ranges);
    cv::Mat nonzeros = my_hist != hist_r;
    std::vector<cv::Point> nonzerosPoints;
    cv::findNonZero(nonzeros, nonzerosPoints);
    std::cout << "nonzeros size = " << nonzerosPoints.size() << std::endl;
    
    int width = 512, height = 500;
    int binWidth = cvRound((double)width / histSize[0]);
    cv::Mat hist = cv::Mat(height, width, CV_8UC3, cv::Scalar(0));
    cv::normalize(hist_b, hist_b, 0, height, cv::NORM_MINMAX, CV_32FC1);
    cv::normalize(hist_g, hist_g, 0, height, cv::NORM_MINMAX, CV_32FC1);
    cv::normalize(hist_r, hist_r, 0, height, cv::NORM_MINMAX, CV_32FC1);
    for (int i = 1; i < histSize[0]; i++) {
        cv::line(hist, cv::Point(binWidth*(i-1), height - cvRound(hist_b.at<float>(i-1))),
                 cv::Point(binWidth*i, height-cvRound(hist_b.at<float>(i))), cv::Scalar(255, 0, 0), 2, cv::LINE_AA, 0);
        cv::line(hist, cv::Point(binWidth*(i-1), height - cvRound(hist_g.at<float>(i-1))),
                 cv::Point(binWidth*i, height-cvRound(hist_g.at<float>(i))), cv::Scalar(0, 255, 0), 2, cv::LINE_AA, 0);
        cv::line(hist, cv::Point(binWidth*(i-1), height - cvRound(hist_r.at<float>(i-1))),
                 cv::Point(binWidth*i, height-cvRound(hist_r.at<float>(i))), cv::Scalar(0, 0, 255), 2, cv::LINE_AA, 0);
    }
    
    cv::normalize(hist_b, hist_b, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    cv::normalize(hist_g, hist_g, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    cv::normalize(hist_r, hist_r, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    cv::Mat hist2 = cv::Mat(300, 256, CV_8UC3, cv::Scalar(0));
    for (int i = 0; i < histSize[0]; i++) {
        cv::line(hist2, cv::Point(i, 299), cv::Point(i, 299-hist_b.at<uchar>(i)), cv::Scalar(128, 0, 0), 1, cv::LINE_AA);
        cv::line(hist2, cv::Point(i, 299), cv::Point(i, 299-hist_g.at<uchar>(i)), cv::Scalar(0, 128, 0), 1, cv::LINE_AA);
        cv::line(hist2, cv::Point(i, 299), cv::Point(i, 299-hist_r.at<uchar>(i)), cv::Scalar(0, 0, 128), 1, cv::LINE_AA);
    }
    
    cv::Mat hsv;
    cv::cvtColor(src, hsv, cv::COLOR_BGR2HSV);
    int hbins = 30, sbins = 32;
    int hsHistSize[] = {hbins, sbins};
    float hranges[] = {0, 180};
    float sranges[] = {0, 256};
    const float* hsranges[] = {hranges, sranges};
    cv::Mat hist3;
    int hschannels[] = {0, 1};
    dims = 2;
    cv::calcHist(&hsv, nimages, hschannels, mask, hist3, dims, hsHistSize, hsranges, uniform, accumulate);
   
    cv::Mat my_hist2 = _calc_hist(hsv, hschannels, dims, hsHistSize, hsranges);
    cv::Mat nonzeros2 = my_hist2 != hist3;
    std::vector<cv::Point> nonzerosPoints2;
    cv::findNonZero(nonzeros2, nonzerosPoints2);
    std::cout << "nonzeros size 2 = " << nonzerosPoints2.size() << std::endl;
    
    cv::normalize(hist3, hist3, 0, 255, cv::NORM_MINMAX, CV_8UC1);
    int scale = 10;
    cv::Mat hist4 = cv::Mat::zeros(hbins*scale, sbins*scale, CV_8UC1);
    for (int i = 0; i < hbins; i++) {
        for (int j = 0; j < sbins; j++) {
            cv::rectangle(hist4, cv::Rect(cv::Point(j*scale, i*scale), cv::Point((j+1)*scale-1, (i+1)*scale-1)), cv::Scalar(hist3.ptr<uchar>(i)[j]), cv::FILLED);
        }
    }
    
    cv::imshow("src", src);
    cv::imshow("hist", hist);
    cv::imshow("hist2", hist2);
    cv::imshow("hist4", hist4);
    cv::waitKey(0);
    
    return 0;
}
